#include"guiapi.h"
#include "rosservergsoap.h"
#include <stdio.h>

RosGuiApi *rosGuiApi;
int main(int argc, char **argv)
{
    ros::init(argc, argv, "roguiapi");

    RosGuiApi myapi;
    rosGuiApi=new RosGuiApi();
    rosGuiApi->init();
    printf("gsoap init\n");
    RosServerGsoap *rosServerGsoap;
    rosServerGsoap=new RosServerGsoap();

    UserInfo userInfo;
    E_MAP_SOLVER mapping_selection;
    E_ODOM_SOLVER odom_strategy;
    E_LOCATION_SOLVER location_strategy;
    E_TRAJECTORY_GLOBAL_SOLVER global_trajectory_algorithm;
    E_TRAJECTORY_LOCAL_SOLVER local_trajectory_algorithm;
    VehiclePosition current_position;
    VehiclePosition position_in;
    position_in.x=2.6;
    position_in.y=1.13;
    position_in.rotate=1.57;

    VehiclePosition initpose;

    initpose.x=-3.5;
    initpose.y=5.0;
    initpose.rotate=0;

    vector<double> polygon_x;
    vector<double> polygon_y;
    polygon_x.push_back(0.4);
    polygon_x.push_back(0.4);
    polygon_x.push_back(0.0);
    polygon_x.push_back(-0.4);
    polygon_x.push_back(-0.4);

    polygon_y.push_back(0.3);
    polygon_y.push_back(-0.3);
    polygon_y.push_back(-0.6);
    polygon_y.push_back(-0.3);
    polygon_y.push_back(0.3);


    std::vector<struct VehiclePosition> positions;
    positions.push_back(position_in);
    positions.push_back(initpose);


    E_GOAL_STATUS moving_status;

    mapping_selection=E_MAP_SOLVER_GMAPPING;//E_MAP_SOLVER_GMAPPING,E_MAP_SOLVER_CARTOGRAPHER,E_MAP_SOLVER_HECTOR
    odom_strategy=E_ODOM_SOLVER_ENCODER;
    location_strategy=E_LOCATION_SOLVER_AMCL;
    global_trajectory_algorithm=E_TRAJECTORY_GLOBAL_SOLVER_ASTAR;
    local_trajectory_algorithm=E_TRAJECTORY_LOCAL_SOLVER_DWA;

//    if (myapi.init() == false)
//    {
//        ROS_ERROR("%s initialization failed", ros::this_node::getName().c_str());
//        return -1;
//    }
//    ROS_INFO("%s initialized", ros::this_node::getName().c_str());
    int robotIdIn=1;
    int returnFlag;


//    //map selecting
//    if(1==myapi.setMapSolver(userInfo, mapping_selection,1,returnFlag))
//    {

//    }
//    else
//    {
//    }

//    // odom selector
//    if(1==myapi.setOdomSolver(userInfo, odom_strategy,returnFlag))
//    {
//        ROS_INFO("odom is luanched \n");
//    }
//    else
//    {
//        ROS_INFO("odom is failed \n");
//    }


//    //map creation
//    if(1==myapi.createMapStart(userInfo,returnFlag))
//    {
//        //printf("mapping online\n");
//    }
//    else
//    {

//        //ROS_INFO("getOdometer error \n");
//    }


//    //map saving
//    if(1==myapi.createMapEnd(userInfo,"first_test",returnFlag))
//    {

//    }
//    else
//    {

//        //ROS_INFO("getOdometer error \n");
//    }


//    //map loading
//    if(1==myapi.loadMap(userInfo,"test01.yaml",returnFlag))
//    {
//        ROS_INFO("map loading done \n");
//    }
//    else
//    {

//        ROS_INFO("map loading failed \n");
//    }


//    if(1==myapi.setTrajectoryGlobalPlanSolver(userInfo, global_trajectory_algorithm,returnFlag))
//    {
//        ROS_INFO("global trajectory is selected \n");
//    }
//    else
//    {
//        ROS_INFO("global trajectory failed \n");

//    }


//    if(1==myapi.setTrajectoryLocalPlanSolver(userInfo, local_trajectory_algorithm,returnFlag))
//    {
//        ROS_INFO("local trajectory is selected \n");
//    }
//    else
//    {
//        ROS_INFO("local trajectory is failed \n");
//    }


//    if(1==myapi.setLocationSolver(userInfo, location_strategy,returnFlag))
//    {
//        ROS_INFO("localization is selected \n");
//    }
//    else
//    {
//        ROS_INFO("localization is failed \n");
//    }


//    if(1==myapi.startNavigation(userInfo, returnFlag))
//    {
//        ROS_INFO("navigation is started \n");
//    }
//    else
//    {
//        ROS_INFO("navigation is failed \n");
//    }


//    if(1==myapi.stopNavigation(userInfo, returnFlag))
//    {
//        ROS_INFO("navigation is stopped \n");
//    }
//    else
//    {
//        ROS_INFO("navigation stop failed \n");
//    }


//    if(1==myapi.changeVelocity(userInfo, 0.255,returnFlag))
//    {
//        ROS_INFO("velocity is set \n");
//    }
//    else
//    {
//        ROS_INFO("velocity setting is failed \n");
//    }


//    if(1==myapi.cancelMotion(userInfo, returnFlag))
//    {
//        ROS_INFO("goal canceled \n");
//    }
//    else
//    {
//        ROS_INFO("goal cancel failed \n");
//    }


//    if(0==myapi.getMovingStatus(userInfo,moving_status))
//    {
//        ROS_INFO("get moving status \n");
//    }
//    else
//    {
//        ROS_INFO("get moving status failed \n");
//    }


//    if(1==myapi.setVehiclePosition(userInfo,initpose,returnFlag))
//    {
//        ROS_INFO("set initial pose \n");
//    }
//    else
//    {
//        ROS_INFO("failed to set initial pose \n");
//    }


    if(1==myapi.moveToPosition(userInfo,position_in,0.3,returnFlag))
    {
        ROS_INFO("move to position \n");
        printf("position %f %f %f \n",position_in.x,position_in.y,position_in.rotate);
    }
    else
    {
        ROS_INFO("move to position failed \n");
        printf("position did not get \n");

    }

//        if(1==myapi.setPositionArrivedAccuracy(userInfo,0.04,0.03,returnFlag))
//        {
//            ROS_INFO("set Position Arrived Accuracy \n");
//        }
//        else
//        {
//            ROS_INFO("failed to set Position Arrived Accuracy \n");
//        }

//        if(1==myapi.setLocalPlanRange(userInfo,8.0,8.0,returnFlag))
//        {
//            ROS_INFO("set Local Plan Range \n");
//        }
//        else
//        {
//            ROS_INFO("failed to set Local Plan Range \n");
//        }

//        if(1==myapi.setLocalBarrierOffset(userInfo,0.5,returnFlag))
//        {
//            ROS_INFO("set Local inflation radius \n");
//        }
//        else
//        {
//            ROS_INFO("failed to set Local inflation radius \n");
//        }

//        //set global inflation radius
//        if(1==myapi.setGlobalBarrierOffset(userInfo,0.5,returnFlag))
//        {
//            ROS_INFO("set global inflation radius \n");
//        }
//        else
//        {
//            ROS_INFO("failed to set global inflation radius \n");
//        }

//        //set robot footprint
//        if(1==myapi.setVehicleBorder(userInfo,polygon_x,polygon_y,returnFlag))
//        {
//            ROS_INFO("set footprint \n");
//        }
//        else
//        {
//            ROS_INFO("failed to set footprint \n");
//        }

//        //add virtual wall
//        if(1==myapi.addVitrualBlock(userInfo,"/home/tcr/virtual_wall.yaml",returnFlag))
//        {
//            ROS_INFO("virtual wall is added \n");
//        }
//        else
//        {
//            ROS_INFO("failed to add virtual wall  \n");
//        }

        //add virtual wall
//        if(1==myapi.deleteVitrualBlock(userInfo,returnFlag))
//        {
//            ROS_INFO("virtual wall is deleted \n");
//        }
//        else
//        {
//            ROS_INFO("failed to delete virtual wall  \n");
//        }

//        //failure repeat num
//        if(1==myapi.setReplanCoutLimitWhenFailure(userInfo,2,returnFlag))
//        {
//            ROS_INFO("failure repeat num is set \n");
//        }
//        else
//        {
//            ROS_INFO("failed to set failure repeat num  \n");
//        }


//        while(ros::ok())
//        {
//            if(1==myapi.getVehiclePosition(userInfo,current_position))
//            {
//                ROS_INFO("get position \n");
//                printf("current_position: %f %f %f\n",current_position.x,current_position.y,current_position.rotate);

//            }
//            else
//            {
//                ROS_INFO("get position failed \n");
//            }
//    }

//        if(1==myapi.moveFollowPath(userInfo,positions,0.3,returnFlag))
//        {
//            ROS_INFO("move to position \n");
//            //printf("position %f %f %f \n",position_in.x,position_in.y,position_in.rotate);
//        }
//        else
//        {
//            ROS_INFO("move to position failed \n");
//            printf("position did not get \n");

//        }
//    while(ros::ok())
//    {
//        rosGuiApi->freeSleep();
//    }

}
